#include "Chassis.h"
#include "../Robotmap.h"
#include "../Commands/DriveWithJoystick.h"

Chassis::Chassis() : Subsystem("Chassis") {
	drive = new RobotDrive(FRONT_LEFT_MOTOR, REAR_LEFT_MOTOR , FRONT_RIGHT_MOTOR, REAR_RIGHT_MOTOR); // Initializes drive train. PWM ports in this order: Front Left, Rear Lefr, Front Right, Rear Right
	drive->SetSafetyEnabled(false);
}
    
void Chassis::InitDefaultCommand() {

	SetDefaultCommand(new DriveWithJoystick()); // When no other chassis command is running the robot will be controlled by Joystick.
}

void Chassis::driveWithJoystick(Joystick *stick, Joystick *stick2)
{
	drive->TankDrive(stick, stick2);
}




